/* Copyright (c) 2007, Jan Flora <janflora@diku.dk>
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 *  - Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 *  - Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *  - Neither the name of the University of Copenhagen nor the names of its
 *    contributors may be used to endorse or promote products derived from this
 *    software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
 * SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
 * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
 * TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

/*
  @author Jan Flora <janflora@diku.dk>
*/

#include <macTypes.h>
#include <macConstants.h>
#include <Int64Compare.h>

module pingM
{
	provides
	{
		interface StdControl;
	}
	uses
	{
	    interface IeeeMacPibAttribute as IeeePibAttribute;
	    interface IeeePanDescriptor;
		interface IeeeMacSdu as IeeeSdu;
		interface IeeeAddress;
	   
		interface McpsDataIndication;
		interface McpsDataRequestConfirm;
	
		interface MlmeAssociateRequestConfirm;
		interface MlmeGtsRequestConfirm;
		interface MlmeGtsIndication;
		interface MlmeScanRequestConfirm;
		interface MlmeSetRequestConfirm;
		interface MlmeSyncRequest;
		interface MlmeSyncLossIndication;
		
		// MCPS
		interface IeeeIndication<Mcps_DataIndication> as McpsIndicationData;   	
		interface IeeeRequestConfirm<Mcps_DataRequestConfirm> as McpsRequestConfirmData;

		// MLME
		interface IeeeRequestConfirm<Mlme_AssociateRequestConfirm> as MlmeRequestConfirmAssociate;
		interface IeeeRequestConfirm<Mlme_GtsRequestConfirm> as MlmeRequestConfirmGts;
		interface IeeeIndication<Mlme_GtsIndication> as MlmeIndicationGts;
		interface IeeeRequestConfirm<Mlme_ScanRequestConfirm> as MlmeRequestConfirmScan;
		interface IeeeSyncRequestConfirm<Mlme_SetRequestConfirm> as MlmeRequestConfirmSet;
		interface IeeeRequest<Mlme_SyncRequest> as MlmeRequestSync;
		interface IeeeIndication<Mlme_SyncLossIndication> as MlmeIndicationSyncLoss;

		interface Debug;
	}
}
implementation
{
	//#define DBG_OFF 0
	#define DBG_LEVEL 1
	#include "Debug.h"
	
	#ifdef PROFILING
		// Set up profiling pins.
		TOSH_ASSIGN_PIN(PROFILING1, C, 5);
		TOSH_ASSIGN_PIN(PROFILING2, C, 6);
		TOSH_ASSIGN_PIN(PROFILING3, C, 7);
		TOSH_ASSIGN_PIN(PROFILING4, G, 3);
		TOSH_ASSIGN_PIN(PROFILING5, G, 1);
		TOSH_ASSIGN_PIN(PROFILING6, B, 7);
		TOSH_ASSIGN_PIN(PROFILING7, B, 3);
		TOSH_ASSIGN_PIN(PROFILING8, B, 2);
		TOSH_ASSIGN_PIN(PROFILING9, B, 1);
	#endif
	
	uint8_t logicalChannel;
	uint16_t myPanId;
	uint8_t myMsduHandle = 0x01;
	Ieee_Address coordAddr;
	uint16_t superframeSpec;
	
	Ieee_Address localAddr;
	Ieee_Msdu myMsdu;
	
	char *payload = "ping";
	
	// Forward declarations of helper functions.
	task void doPing();
	task void doScan();
	task void associate();
	task void sync();
	task void allocateGts();
	void processScan(Mlme_ScanRequestConfirm scanconfirm);
	void setCoordAddrInfo();
	void setBeaconOrder();
	void setSuperframeOrder();
	void setAutoRequest(bool autoRequest);
	result_t setAttribute(Ieee_PibAttribute attribute);
	int strcmp(const char * a, const char * b);
	
	command result_t StdControl.init()
	{
		#ifdef PROFILING
			// Setup profiling pins as output pins.
			TOSH_MAKE_PROFILING1_OUTPUT();
			TOSH_MAKE_PROFILING2_OUTPUT();
			TOSH_MAKE_PROFILING3_OUTPUT();
			TOSH_MAKE_PROFILING4_OUTPUT();
			TOSH_MAKE_PROFILING5_OUTPUT();
			TOSH_MAKE_PROFILING6_OUTPUT();
			TOSH_MAKE_PROFILING7_OUTPUT();
			TOSH_MAKE_PROFILING8_OUTPUT();
			TOSH_MAKE_PROFILING9_OUTPUT();
			
			// Clear all pins.
			TOSH_CLR_PROFILING1_PIN();
			TOSH_CLR_PROFILING2_PIN();
			TOSH_CLR_PROFILING3_PIN();
			TOSH_CLR_PROFILING4_PIN();
			TOSH_CLR_PROFILING5_PIN();
			TOSH_CLR_PROFILING6_PIN();
			TOSH_CLR_PROFILING7_PIN();
			TOSH_CLR_PROFILING8_PIN();
			TOSH_CLR_PROFILING9_PIN();
		#endif
		
		return SUCCESS;
	}

	command result_t StdControl.start()
	{
		uint8_t myAddr[8] = {0x1D, 0x00, 0x01, 0xB0, 0x37, 0xC2, 0x50, 0x00};
		memcpy(aExtendedAddress, myAddr, 8);
		post doScan();
		return SUCCESS;
	}

	command result_t StdControl.stop()
	{
		return FAIL;
	}
	
	/*******************************
	  Data handling        
	 *******************************/
	
	event void McpsIndicationData.indication(Mcps_DataIndication indication)
	{
		char *data;
		uint8_t dataLen;
		
		#ifdef PROFILING
			TOSH_SET_PROFILING1_PIN();
		#endif
		
		call IeeeSdu.create(&myMsdu);
		call McpsDataIndication.getMsdu(indication, myMsdu);
		data = (char*)call IeeeSdu.getPayload(myMsdu);
		dataLen = call IeeeSdu.getPayloadLen(myMsdu);
		
		if (dataLen == 5 && 0 == strcmp("pong", data)) {
			DBG_STR("Received a ping reply.",1);
		} else {
			DBG_STR("Ignoring data received different from ping reply.",1);
			DBG_DUMP(data, dataLen, 1);
		}

		call McpsDataIndication.destroy(indication);
		call IeeeSdu.destroy(myMsdu);
		
		#ifdef PROFILING
			TOSH_CLR_PROFILING1_PIN();
		#endif
	}

	event void McpsRequestConfirmData.confirm( Mcps_DataRequestConfirm confirm )
	{
		//uint8_t handle = call McpsDataRequestConfirm.getMsduHandle(confirm);
		Ieee_Status status = call McpsDataRequestConfirm.getStatus(confirm);
		
		if (status == IEEE802154_SUCCESS) {
			DBG_STR("Ping transmitted.",1);
		} else {
			DBG_STR("Ping failed.",1);
		}
		call McpsDataRequestConfirm.destroy(confirm);
		
		#ifdef PROFILING
			TOSH_CLR_PROFILING2_PIN();
		#endif
	}
		
	task void doPing()
	{
		Mcps_DataRequestConfirm request;

		#ifdef PROFILING
			TOSH_SET_PROFILING2_PIN();
		#endif

		call IeeeSdu.create( &myMsdu );
    
		// Create request
		call McpsDataRequestConfirm.create(&request, localAddr, coordAddr, 5);

		call McpsDataRequestConfirm.getMsdu( request, myMsdu );		
		memcpy(call IeeeSdu.getPayload( myMsdu ), payload, 5);
		call McpsDataRequestConfirm.setMsduHandle( request, myMsduHandle );
		call McpsDataRequestConfirm.setTxOptions( request, IEEE802154_TxOptsAck | 0x02);

		if (SUCCESS == call McpsRequestConfirmData.request(request)) {
			DBG_STR("Ping was enqueued.",1);
			myMsduHandle++;
		} else {
			DBG_STR("Transmission of ping failed.",1);
		}
		
		// Clean up.
		call IeeeSdu.destroy( myMsdu );
	}
	
	/*******************************
	  GTS handling        
	 *******************************/

	event void MlmeIndicationGts.indication( Mlme_GtsIndication indication )
  	{
  		uint8_t gtsChars;
  		
  		#ifdef PROFILING
  			TOSH_SET_PROFILING3_PIN();
  		#endif
  		
  		gtsChars = call MlmeGtsIndication.getGtsCharacteristics(indication);
  		
  		if (gtsChars & 0x08) {
  			DBG_STR("My receive GTS was deallocated by the coordinator", 1);
  		} else {
  			DBG_STR("My transmit GTS was deallocated by the coordinator", 1);
  		}
  		
  		call MlmeGtsIndication.destroy(indication);
  		
  		#ifdef PROFILING
  			TOSH_CLR_PROFILING3_PIN();
  		#endif
  	}

	event void MlmeRequestConfirmGts.confirm(Mlme_GtsRequestConfirm confirm)
	{
		Ieee_Status status = call MlmeGtsRequestConfirm.getStatus(confirm);

		if (status == IEEE802154_SUCCESS) {
			DBG_STR("GTS allocated successfully",1);
			post doPing();
		} else {
			DBG_STRINT("GTS allocation failed with status: ",status,1);
		}
		call MlmeGtsRequestConfirm.destroy(confirm);
		
		#ifdef PROFILING
			TOSH_CLR_PROFILING4_PIN();
		#endif
	}
	
	task void allocateGts()
	{
		Mlme_GtsRequestConfirm request;
		
		#ifdef PROFILING
			TOSH_SET_PROFILING4_PIN();
		#endif
		
		call MlmeGtsRequestConfirm.create(&request);
		call MlmeGtsRequestConfirm.combineGtsCharacteristics(request,1,0,1);
		
		if (call MlmeRequestConfirmGts.request(request)) {
			DBG_STR("Requesting guaranteed timeslot",1);
		} else {
			DBG_STR("Could not request guaranteed timeslot",1);
		}
	}
	
	/*******************************
	  Association handling        
	 *******************************/

	event void MlmeRequestConfirmAssociate.confirm(Mlme_AssociateRequestConfirm confirm)
	{
		uint16_t myShortAddr;
		Ieee_Status status = call MlmeAssociateRequestConfirm.getStatus(confirm);
		
		if (status == IEEE802154_SUCCESS) {
			myShortAddr = call MlmeAssociateRequestConfirm.getAssocShortAddress(confirm);
			// Create an address ADT.
			call IeeeAddress.create( &localAddr );
			call IeeeAddress.setAddrMode( localAddr, 2 );
			call IeeeAddress.setAddress( localAddr, (uint8_t*)&myShortAddr );
			call IeeeAddress.setPanId( localAddr, myPanId);
			DBG_STR("Associated to PAN.",1);
			post allocateGts();
		} else {
			DBG_STR("Association failed.",1);
			DBG_STRINT("Status was: ",status,1);
		}
		call MlmeAssociateRequestConfirm.destroy(confirm);
		
		#ifdef PROFILING
			TOSH_CLR_PROFILING5_PIN();
		#endif
	}
	
	task void associate()
	{
		Mlme_AssociateRequestConfirm request;
		
		#ifdef PROFILING
			TOSH_SET_PROFILING5_PIN();
		#endif
		
		call MlmeAssociateRequestConfirm.create(&request, coordAddr);
		call MlmeAssociateRequestConfirm.setLogicalChannel(request, logicalChannel);
		call MlmeAssociateRequestConfirm.setCapabilityInformation(request,IEEE802154_CapInfoAllocAddr);
		
		if (call MlmeRequestConfirmAssociate.request(request))
		{
			DBG_STR("Associating to PAN",1);
		} else {
			DBG_STR("Could not request association to PAN...",1);
		}
	}
	
	
	/*******************************
	  Scan handling        
	 *******************************/
	
	event void MlmeRequestConfirmScan.confirm(Mlme_ScanRequestConfirm confirm)
	{			
		Ieee_Status status = call MlmeScanRequestConfirm.getStatus(confirm);
		uint8_t scanType = call MlmeScanRequestConfirm.getScanType(confirm);		
			
 		if (scanType == IEEE802154_PassiveScan && status == IEEE802154_SUCCESS) {
 			processScan(confirm);
		} else {
			DBG_STR("Scan failed.",1);
		}
		call MlmeScanRequestConfirm.destroy(confirm);
		
		#ifdef PROFILING
			TOSH_CLR_PROFILING6_PIN();
		#endif
	}
	
	void processScan(Mlme_ScanRequestConfirm scanconfirm)
	{
		uint8_t linkQuality,bestLinkQuality = 0;
		uint8_t i,listSize;
		uint16_t sframeSpec;
		Ieee_PanDescriptor pd,bestPd = NULL;
		
		listSize = call MlmeScanRequestConfirm.getResultListSize(scanconfirm);
		DBG_STRINT("Examining :",listSize,1);
		
		// Find best PAN coordinator.
		for (i = 0; i < listSize; i++) {
			pd = call MlmeScanRequestConfirm.getPanDescriptor(scanconfirm,i);
			sframeSpec = call IeeePanDescriptor.getSuperframeSpec(pd);

			// TODO: change this to be freescale independent
/*			if ( (sframeSpec & gSuperFrameSpecMsbAssocPermit_c) &&
			     (sframeSpec>>8 & gSuperFrameSpecLsbBO_c) == 0x7 ) {*/
			     linkQuality = call IeeePanDescriptor.getLinkQuality(pd);
				// Get the best
				if (linkQuality >= bestLinkQuality) {
					bestLinkQuality = linkQuality;
					bestPd = pd;
				}
//			}
		}

		if (bestPd) {
			DBG_STR("Found PAN!",1);
			call IeeeAddress.create(&coordAddr);
			call IeeePanDescriptor.getCoordAddr(bestPd, coordAddr);
			myPanId = call IeeeAddress.getPanId(coordAddr);
			DBG_STRINT("PAN Id is:",myPanId,1);
			logicalChannel = call IeeePanDescriptor.getLogicalChannel(bestPd);
			superframeSpec = call IeeePanDescriptor.getSuperframeSpec(bestPd);
			
			// Get ready to synchronize with the coordinator.
			setCoordAddrInfo();
			setBeaconOrder();
			setSuperframeOrder();
			// Make device retrieve indirect data automatically
			// when indicated in the beacon.
			setAutoRequest(TRUE);
			post sync();
		} else {
			DBG_STR("No PAN.",1);
		}
	}
	
	task void doScan()
	{
		Mlme_ScanRequestConfirm request;
		
		#ifdef PROFILING
			TOSH_SET_PROFILING6_PIN();
		#endif
			
		// Create the scan request
		// Scan channels 12 - 15
		call MlmeScanRequestConfirm.create(&request);
		call MlmeScanRequestConfirm.setScanType(request,IEEE802154_PassiveScan);
		call MlmeScanRequestConfirm.setScanChannels(request,0x0000F000);
		call MlmeScanRequestConfirm.setScanDuration(request,0x8);
		
		if (call MlmeRequestConfirmScan.request(request))
		{
			DBG_STR("Starting scan...",1);
		}
		else
		{
			DBG_STR("Could not start scan...",1);
		}
	}
	
	/*******************************
	  Synchronization handling
	 *******************************/
	
	event void MlmeIndicationSyncLoss.indication( Mlme_SyncLossIndication indication)
	{
		Ieee_Status reason = call MlmeSyncLossIndication.getLossReason(indication);
		DBG_STRINT("Synchronization lost. Reason: ",reason,1);
		call MlmeSyncLossIndication.destroy(indication);
	}
	
	task void sync()
	{
		Mlme_SyncRequest request;

		#ifdef PROFILING
			TOSH_SET_PROFILING7_PIN();
		#endif

		//create sync request
		call MlmeSyncRequest.create( &request );
		call MlmeSyncRequest.setLogicalChannel(request, logicalChannel);
		call MlmeSyncRequest.setTrackBeacon(request, TRUE);
		
		//do request
		if (SUCCESS == call MlmeRequestSync.request( request )) {
			DBG_STR("Beacon tracking requested!",1);
			post associate();
		} else {
			DBG_STR("Beacon tracking request failed!",1);
		}
		
		#ifdef PROFILING
			TOSH_CLR_PROFILING7_PIN();
		#endif
	}
	
	/*******************************
	  PIB attribute handling
	 *******************************/
	
	void setCoordAddrInfo()
	{
		Ieee_PibAttribute attribute;
		
		call IeeePibAttribute.create(8,&attribute);
		
		if (call IeeeAddress.getAddrMode(coordAddr) == 2) {
			uint16_t myAddr;
			call IeeeAddress.getAddress(coordAddr, (uint8_t*)&myAddr);
			call IeeePibAttribute.setMacCoordShortAddress(attribute,myAddr);
		} else {
			uint8_t myAddr[8];
			call IeeeAddress.getAddress(coordAddr, myAddr);
			call IeeePibAttribute.setMacCoordExtendedAddress(attribute,myAddr);
		}
		// Set coordinator address
		// setAttribute is responsible for destroying the primitive.
		setAttribute(attribute);
		
		call IeeePibAttribute.create(2,&attribute);
		call IeeePibAttribute.setMacPanId(attribute,call IeeeAddress.getPanId(coordAddr));

		// setAttribute is responsible for destroying the primitive.
		setAttribute(attribute);
	}
	
	void setBeaconOrder()
	{
		Ieee_PibAttribute attribute;
		
		call IeeePibAttribute.create(1,&attribute);
		call IeeePibAttribute.setMacBeaconOrder(attribute,(superframeSpec&0x000F));

		// setAttribute is responsible for destroying the primitive.
		setAttribute(attribute);
	}
	
	void setSuperframeOrder()
	{
		Ieee_PibAttribute attribute;
		
		call IeeePibAttribute.create(1,&attribute);
		call IeeePibAttribute.setMacSuperframeOrder(attribute,(superframeSpec&0x00F0)>>4);
		
		// setAttribute is responsible for destroying the primitive.
		setAttribute(attribute);
	}

	void setAutoRequest(bool autoRequest)
	{
		Ieee_PibAttribute attribute;
		
		call IeeePibAttribute.create(1,&attribute);
		call IeeePibAttribute.setMacAutoRequest(attribute,autoRequest);
		
		// setAttribute is responsible for destroying the primitive.
		setAttribute(attribute);
	}
	
	result_t setAttribute(Ieee_PibAttribute attribute)
	{
		Mlme_SetRequestConfirm request;
		Ieee_Status status;
		call MlmeSetRequestConfirm.create(&request);
		call MlmeSetRequestConfirm.setPibAttribute(request,attribute);
		request = call MlmeRequestConfirmSet.request(request);
		
		status = call MlmeSetRequestConfirm.getStatus(request);
		if (SUCCESS != call IeeePibAttribute.destroy(attribute)) {
			DBG_STR("Could not destroy attribute",3);
		}
		call MlmeSetRequestConfirm.destroy(request);
		if (status != IEEE802154_SUCCESS) {
			DBG_STR("Set operation was unsuccessful!",1);
			return FAIL;
		}
		return SUCCESS;
	}
	
	event void MlmeRequestConfirmSet.confirm(Mlme_SetRequestConfirm confirm)
	{
		// We don't do anything here as we handle set requests in sync.
	}
	
	/** Help function, does string compare */
	int strcmp(const char * a, const char * b)
	{
		while (*a && *b && *a == *b) { ++a; ++b; };
		return *a - *b;
	}
}